#ifndef PLANNER_H
#define PLANNER_H

#include "Scene.h"
#include <QPointF>
#include <QVector>
#include <QPolygonF>
#include "basic_typedef.h"
#include "Sampler.h"
#include "CollisionDetector.h"
#include "Prm.h"

class Planner {
public:     
  ////////////////////////
  // C'tors & D'tors
  ////////////////////////

  /* The constructor transforms the input data (obstacles, query, and robot
   * radius) from the GUI to CGAL objects. 
   */
  Planner(Scene* scene): m_scene(scene), 
                         m_radius(scene->getRadius()),
                         m_robot_num(scene->getStartPositions().size())
  {
    // transform data from Scene
    QVector<QPolygonF> obstacles = m_scene->getObstacles();
    foreach (QPolygonF qp, obstacles) {
      Polygon_2 cp;
      for (int j=0; j < qp.size(); ++j) {
        cp.push_back(transformToCGAL(qp[j]));
      }
      m_obstacles.push_back(cp);
    }
        
    QVector<QVector<QPointF> > startPositions = m_scene->getStartPositions();
    QVector<QVector<QPointF> > targetPositions = m_scene->getTargetPositions();

    m_start_point = transformToCGAL(startPositions[0][0]);
    m_target_point = transformToCGAL(targetPositions[0][0]);

    Point_2 ptl = transformToCGAL(m_scene->getRoomTopLeft());
    Point_2 pbr = transformToCGAL(m_scene->getRoomBottomRight());
    m_room = Room(ptl,pbr);
  }

  ~Planner()
  {
    delete m_collision;
    delete m_sampler;
  }

  /* This function is called by the GUI when you press the "Execute" button. 
   * You can change the structure of Prm.h, but remember 
   * to run the "tranform_path" function to update the gui
   */
  void run()
  {
    m_collision = new CollisionDetector(m_radius, &m_obstacles);
    m_sampler = new Sampler(m_radius, m_room, m_collision);
                
    // An example 
    Prm roadmap = Prm(300, 12, m_collision,
    m_sampler, m_start_point, m_target_point);
    roadmap.generate_roadmap();

    // retrieve path from PRM
    vector<Point_2> path = roadmap.retrieve_path();

    // transform path to GUI and update the display in gui
    transfrom_path(path);
  }

private:
  /////////////////
  // Procedures that transform CGAL objects to GUI and vice versa
  /////////////////

  Point_2 transformToCGAL(QPointF qp)
  {
    // x and y coordinates of the point
    double x = qp.x();
    double y = qp.y();

    Point_2 cpoint(x,y);
    return cpoint;
  }

  QPointF transformToQT(Point_2 cp)
  {
    return QPointF(cp.x() , cp.y());
  }

  QVector<QPointF> transformToQT(PointSet cs)
  {
    QVector<QPointF> result;
    foreach (Point_2 c, cs) {
      result.push_back(transformToQT(c));
    }
    return result;
  }

  QVector<QVector<QPointF> > transformToQT(Path path)
  {
    QVector<QVector<QPointF> > result;

    foreach(PointSet cs, path)
    {
      result.push_back(transformToQT(cs));
    }
    return result;
  }

  void transfrom_path(vector<Point_2>& path)
  {
    vector<vector<Point_2> > multi_path;
    foreach (Point_2 p, path) {
      vector<Point_2> dummy;
      dummy.push_back(p);
      multi_path.push_back(dummy);
    }

    // path section number - group - info
    vector<vector<pair<QTPath,GeometricGraph> > > result;
                
    vector<pair<Point_2,Point_2> > dummyPairVector;
    PointSet                dummyCS;
    GeometricGraph dummyGG(dummyPairVector, dummyCS);

    foreach (PointSet cs, multi_path) {
      vector<pair<QTPath,GeometricGraph> > path_graph_section;
      vector<PointSet> strip_cs;
      foreach (Point_2 c, cs) {
        PointSet cs2;
        cs2.push_back(c);
        strip_cs.push_back(cs2);
      }

      foreach(PointSet group_cs, strip_cs)
      {
        list<PointSet> dummyPath;
        dummyPath.push_back(group_cs);
        QVector<QVector<QPointF> > qt_path = transformToQT(dummyPath);
        pair<QTPath,GeometricGraph> p(qt_path, dummyGG);
        path_graph_section.push_back(p);
      }
      result.push_back(path_graph_section);
    }

    m_scene->setPath(result);
  }

  double path_legth(vector<vector<Point_2> > path)
  {
    double length = 0;
    for (size_t i = 1; i < path.size(); ++i) {
      for (size_t r = 0; r < m_robot_num; ++r) {
        length += pow(path[i][r].x() - path[i-1][r].x(),2) + 
          pow(path[i][r].y() - path[i-1][r].y(),2);
      }
    }
    return length;
  }

  ///////////////////////
  // Data members
  ///////////////////////

  /*        Scene is the interface class between the GUI and the planner.
            The extraction of the scenario data is done through this 
            object. The result of the planner (the path) is returned
            using this object as well.        */
  Scene* m_scene;                
  double m_radius;                // Radius of the robot
  Obstacles m_obstacles;// Obstacles, represented by polygons
  size_t m_robot_num;        // obsolete 
  Point_2 m_start_point, m_target_point; // Start/target configurations
  Room m_room;        // Boundaries of the scenario

  CollisionDetector* m_collision;    //        Collision detection black box
  Sampler* m_sampler;                //        Sampler object
};

#endif
